#include "public.h"
#include "control.h"

/******************************D E F I N I T I O N ****************************/
//===============================数据采集================================//

// 拨码开关状态
int STATUS_1 = 0;
int STATUS_2 = 0;
int STATUS_3 = 0;
int STATUS_4 = 0;

// run参数
int g_speed_final = 0; // 速度给定
int g_speed_integral = 0;
int g_speed_p = 0;
int g_speed_i = 0;
int g_speed_d = 0;
int speed_feedback = 0;
int speed_error = 0;
int speed_Derror = 0;
int speed_PreError = 0;

// turn参数
int bias = 0;
int g_direction_p = 0;
int g_direction_d = 0;

// 左右轮脉冲计数
int g_feedback_l = 0;
int g_feedback_r = 0;

// 车控制量
CarControll carcon;

// 计时时间
int g_time = 0;

//入库
extern int banma_flag;
int ruku_time=0;

/******************************************************************************/
/******************************F U N C T I O N S  *****************************/
/******************************************************************************/

/******************************************************************************/
void Control(void) // 4ms控制中断
{
    g_feedback_r = -Count_R(); // 获取对管码盘返回值
    g_feedback_l = Count_L();

    // 控制量计算
    Car_Run();  // 速度控制
    Car_Turn(); // 转向控制

    // 电机赋值
    Motor_Set(); // 电机赋P波

    g_time++; // 中断计时
		
	if(banma_flag)
		ruku_time++;
}

/*
function name:  running_init
description:    init

变量初始化

*/
void Running_Init(void)
{
    // 拨码开关状态获取
    STATUS_1 = Jump_1();
    STATUS_2 = Jump_2();
    STATUS_3 = Jump_3();
    STATUS_4 = Jump_4();

  // 电机PID参数
    g_speed_final = 30;
    g_speed_p = 57;
    g_speed_i = 6;
    g_speed_d = 3;

    // 转向pd参数
    g_direction_p = 80;
    g_direction_d = 8;

}
// 位置式pid
void Car_Run(void)
{

    if (g_time < 300)
    {
        carcon.run_set = 0; // 前1.5秒不给速度
    }
    else
    {
        carcon.run_set = g_speed_final;

        speed_feedback = (g_feedback_l + g_feedback_r) / 2;

        speed_error = carcon.run_set - speed_feedback; // 计算速度偏差 error
        g_speed_integral += speed_error;

        // 积分限幅！！！
        if (g_speed_integral > 300)
            g_speed_integral = 300;
        else if (g_speed_integral < -300)
            g_speed_integral = -300;

        speed_Derror = speed_error - speed_PreError;
        speed_PreError = speed_error;

        carcon.run = speed_error * g_speed_p + g_speed_integral * g_speed_i + speed_Derror * g_speed_d;
    }
}

/*
function name:  car_turn
parameters:     none
author:         LiTianHui
date:           2022-8-6
description:    control car turn
*/
void Car_Turn(void)
{

    bias = -MID_VIDEO_POINT + g_DirectionControl;

    float anglev = g_feedback_r - g_feedback_l;

    carcon.turn = bias * g_direction_p + anglev * g_direction_d; // 计算转向值
}

void Motor_Set(void)
{
    carcon.motor_left = carcon.run + carcon.turn;
    carcon.motor_right = carcon.run - carcon.turn;

    if (carcon.motor_left > 9000)
    {
        carcon.motor_left = 9000;
    }
    else if (carcon.motor_left < -9000)
    {
        carcon.motor_left = -9000;
    }

    if (carcon.motor_right > 9000)
    {
        carcon.motor_right = 9000;
    }
    else if (carcon.motor_right < -9000)
    {
        carcon.motor_right = -9000;
    }
		if(banma_flag)
		{
			if(STATUS_1==1)
			{
				if(ruku_time<140)
				{
					Setpwm_L(-4000);
					Setpwm_R(-100);
				}
				while(ruku_time>=140)
				{
					Setpwm_L(0);
					Setpwm_R(0);
				}
				
			}
			else if(STATUS_1==0)
			{
				if(ruku_time<140)
				{
					Setpwm_R(-4000);
					Setpwm_L(-100);
				}
				while(ruku_time>=140)
				{
					Setpwm_L(0);
					Setpwm_R(0);
				}
				
			}
		}
		else
		{
			if(g_time>=300&&g_time<530)
			{
					Setpwm_L(-1200);
					Setpwm_R(-1200);
			}
			else if(g_time>=530&&g_time<630)
			{
				if(STATUS_1==1){
					Setpwm_L(-3500);
					Setpwm_R(-100);
				}
				else if(STATUS_1==0){
					Setpwm_R(-3500);
					Setpwm_L(-100);
				}
			}
			else{
				Setpwm_L(-carcon.motor_left);
				Setpwm_R(-carcon.motor_right); // 给电机赋P波
			}
	
		}



}
